#ifndef _PID_H
#define _PID_H

class PID_Controller
{
public:
    PID_Controller() {}
    PID_Controller(float _Kp, float _Ki, float _Kd) : Kp(_Kp), Ki(_Ki), Kd(_Kd) {}
    void SetPIDParam(float _Kp, float _Ki, float _Kd, float _I_Term_Max, float _Out_Max)
    {
        Kp = _Kp;
        Ki = _Ki;
        Kd = _Kd;
        I_Term_Max = _I_Term_Max;
        Out_Max = _Out_Max;
    };
    float Adjust();
    float Target = 0, Current = 0, Error = 0;
    float Out = 0;

    float Kp = 0, Ki = 0, Kd = 0;
    float I_Term_Max = 0; /*<! I项限幅 */
    float Out_Max = 0;    /*<! 输出限幅 */

    float I_Term = 0; /* 积分器输出 */
    float P_Term = 0; /* 比例器输出 */
    float D_Term = 0; /* 微分器输出 */

    float I_SeparThresh = 400; /*!< 积分分离阈值，需为正数。std::abs(error)大于该阈值取消积分作用。*/

    float DeadZone = 0; /*!< 死区，需为整数，std::abs(error)小于DeadZone时，输出为0。 */
private:
    float pre_error = 0;
    float integral_e = 0;

    float pre_Current = 0;

    float output_constrain(float _val,float _min,float _max);
};

#endif

